import rclpy
from rclpy.node import Node
from rclpy.logging import get_logger
from tf2_ros.static_transform_broadcaster  import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations
import sys
class TFStaticBroadcastPyNode(Node):
    def __init__(self,argv:list):
        super().__init__('TF_static_broadcast_py_node')
        print(list)
        self.get_logger().info('TFStaticBroadcastPyNode创建')
        self.static_broadcast: StaticTransformBroadcaster =StaticTransformBroadcaster(self)
        self.send_transform(argv)
    def send_transform(self,argv:list):
        static_broadcast_: StaticTransformBroadcaster=self.static_broadcast
        ts: TransformStamped=TransformStamped()
        ts.header.frame_id=argv[8]
        ts.header.stamp=self.get_clock().now().to_msg();
        #
        ts.child_frame_id=argv[7]
        #
        ts.transform.translation.x=float(argv[1])
        ts.transform.translation.y=float(argv[2])
        ts.transform.translation.z=float(argv[3])
        #
        qtn: tuple=tf_transformations.quaternion_from_euler(float(argv[4]),float(argv[5]),float(argv[6]))
        ts.transform.rotation.x=qtn[0]
        ts.transform.rotation.y=qtn[1]
        ts.transform.rotation.z=qtn[2]
        ts.transform.rotation.w=qtn[3]
        #
        static_broadcast_.sendTransform(ts)
        
def main():
    # run py03_tf_broadcaster demo01_static_tf_broadcast_py
    # 
    if len(sys.argv)!=9:
        get_logger('rclpy').error('请传入9个参数')
        return 1
    rclpy.init()
    rclpy.spin(TFStaticBroadcastPyNode(sys.argv))
    rclpy.shutdown()


if __name__ == '__main__':
    main()
